/*
 * pid_controller.c
 *
 *  Created on: 10 sept. 2014
 *      Author: MarioC
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "pll/pid_controller.h"


void pid_init(pid_ctrl_t* pid, pid_params_t *pid_params, pid_limiter_t *limiter)
{

	if(NULL != pid && NULL != pid_params && NULL != limiter){
	    memcpy(&pid->params, pid_params, sizeof(pid_params_t));

	    // Initialize integral and derivative calculations
	    pid->integral_buffer = (float)pid->params.set_point;

	    pid->limiter.max = limiter->max;
	    pid->limiter.min = limiter->min;
	    pid->prev_result = 0;
	}
}



uint32_t pid_update(pid_ctrl_t* pid, int32_t error, const pid_limiter_t* pid_limiter)
{
  uint32_t result = 0;


  if(NULL != pid){
	  double	ki;
	  double	tmp;
	  double    kp;
	  double    intpart;

	  s3_8_t_to_double(&ki, &pid->params.ki);
	  pid->integral_buffer += (ki * (double)error);

	  s3_8_t_to_double(&kp, &pid->params.kp);
	  tmp = (kp * error) + pid->integral_buffer;


	  modf (tmp , &intpart);
	  result = (uint32_t)ceil(intpart);

	  if (result > pid->limiter.max) {
	      result = pid->limiter.max;
	  }
      if (result < pid->limiter.min) {
          result = pid->limiter.min;
      }

      // anti wind up option
      if (true == pid->params.anti_windup) {
          pid->integral_buffer = - tmp + (float)pid->prev_result;;
      }
      pid->prev_result = result;

  }

  return(result);
}

void
float_to_s3_8_t(s3_8_t *out, float in)
{
	if (NULL != out) {
		double fractpart, intpart;
		out->raw = 0;			// default

		fractpart = modf (in , &intpart);
		if (intpart <= 7) {
			out->bits_t.int_part = (uint8_t)intpart;

			fractpart *= 255;
			fractpart = modf (fractpart , &intpart);
			out->bits_t.fract_part = (((uint8_t)intpart+1) & 0xFF);
		}
	}
}


void
s3_8_t_to_double(double *out, s3_8_t *in)
{
	if (NULL != in && NULL != out) {
		*out = in->bits_t.int_part + ((double)in->bits_t.fract_part)/256.0;
	}
}
